package monitor;

import i_grafica.FuncionalidadD;
import i_grafica.Principal;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Iterator;

import javax.vecmath.Point3d;

import comunicacion.ParametrosEntorno;

import estrategia.Controlador;
import estrategia.ParametrosGlobales;
import estrategia.Utiles;

/**
 * 
 * Estaria bueno que las demas operaciones logicas, tuvieran la opcion de pausarse
 * de esta manera, el algoritmo puede cobrar relevancia frente a las otras cosas 
 * 
 * */
public class DetectorAtascamientoD extends Thread {
	private boolean vivo = true;
	private static boolean robot_vivo = true;
	private Point3d mi_posicion;
//	private Point3d posicion_otros[];
	private Controlador control;
	private Utiles u;
	private ManagerObjetos managerObjetos;
	
	private boolean mate_enemigo = false;
	private boolean me_mataron = false;
	private int cuadrante_mate_enemigo;
	
	public DetectorAtascamientoD(Controlador c){		
		control = c;
		managerObjetos = c.managerObjetos;
		u = new Utiles(control.entorno);
	}
	
	/*seters*/
	public void killThread() { vivo = false; }
	public static void murioRobot() { robot_vivo = false; }

	/*geters*/
	public boolean threadVivo() { return vivo; }
	public static boolean robotVivo() { return robot_vivo; }
	
	
	private void colisionPared() throws IOException{
		
		//Obtengo posicion de mi robot.
		double distancia_pared_delantera = ParametrosEntorno.POSICION_PARED_DELANTERA - mi_posicion.y; 
		double distancia_pared_trasera =  mi_posicion.y - ParametrosEntorno.POSICION_PARED_TRASERA;
		double distancia_pared_izquierda =  mi_posicion.x - ParametrosEntorno.POSICION_PARED_IZQUIERDA;
		double distancia_pared_derecha = ParametrosEntorno.POSICION_PARED_DERECHA - mi_posicion.x;
			
		//Si estoy muy proximo a la pared, y perpendicular, salgo.
		
		if ( distancia_pared_delantera < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, Math.PI/2)||
				distancia_pared_trasera < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, 3*Math.PI/2) ||
				distancia_pared_izquierda < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, Math.PI) ||
				distancia_pared_derecha < ParametrosGlobales.TOLERANCIA_PARED  && angulosIguales(mi_posicion.z, 0)) {
			
			control.entorno.enviarBeep();
			
			//Pauso tactica actual										
			//pausarDemonios();
			pausarEstrategia();
			
			//Retrocedo
			control.retroceder(ParametrosGlobales.VELOCIDAD_ESCAPE);
			try {
				sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			control.detenerMotores();
			//Giro
			double angulo_giro = (mi_posicion.z + ParametrosGlobales.ANGULO_ESCAPE) % (2*Math.PI);
			control.girar(angulo_giro);
			
			//Reanudamos la tactica actual
			//reactivarDemonios();
			reanudarEstrategia();
			
		} else if  ( distancia_pared_delantera < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, 3*(Math.PI/2))||
				distancia_pared_trasera < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, Math.PI/2) ||
				distancia_pared_izquierda < ParametrosGlobales.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, 0) ||
				distancia_pared_derecha < ParametrosGlobales.TOLERANCIA_PARED  && angulosIguales(mi_posicion.z, Math.PI)) {
			
			control.entorno.enviarBeep();
			
			//Pauso tactica actual										
			//matarDemonios();
			pausarEstrategia();
			
			//Retrocedo
			control.avanzar(ParametrosGlobales.VELOCIDAD_ESCAPE);
			try {
				sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			control.detenerMotores();
			
			//Reanudamos la tactica actual
			//reactivarDemonios();
			reanudarEstrategia();
		}
	}
	
	/**
	 * Devuelve si 2 angulos son iguales, tomando en cuenta cierto margen de angulo
	 * */
	public boolean angulosIguales(double angulo1, double angulo2) {
		angulo1 = angulo1 % (2*Math.PI);
		angulo2 = angulo2 % (2*Math.PI);
		double distancia_cos = Math.abs(Math.cos(angulo1) - Math.cos(angulo2));
		double distancia_sin = Math.abs(Math.sin(angulo1) - Math.sin(angulo2)) ;
		if( distancia_cos < ParametrosGlobales.TOLERANCIA_ANGULOS_OPUESTOS_COS &&
				distancia_sin < ParametrosGlobales.TOLERANCIA_ANGULOS_OPUESTOS_SIN)
			return true;
		
		return false;
	}
	
	/**
	 * Devuelve si 2 angulos son iguales, tomando en cuenta el angulo que se le pasa
	 * */
	public boolean angulosIguales(double angulo1, double angulo2, double angulo_tol) {
		angulo1 = angulo1 % (2*Math.PI);
		angulo2 = angulo2 % (2*Math.PI);
		double diferencia = u.getDiferenciaAngulos(angulo1, angulo2);	
		if( diferencia < angulo_tol )
			return true;
		
		return false;
	}
	
	/**
	 * Devuelve si 2 angulos osn iguales, teniendo en cuenta las tolerancias que se le pasa como parametro
	 * */
	public boolean angulosIguales(double angulo1, double angulo2, double tol_sin, double tol_cos) {
		angulo1 = angulo1 % (2*Math.PI);
		angulo2 = angulo2 % (2*Math.PI);
		double distancia_cos = Math.abs(Math.cos(angulo1) - Math.cos(angulo2));
		double distancia_sin = Math.abs(Math.sin(angulo1) - Math.sin(angulo2)) ;
		if( distancia_cos < tol_cos && distancia_sin < tol_sin)
			return true;
		
		return false;
	}
	
	public static double normalizarAngulo(double angulo) {
		double res = angulo % (2*Math.PI);
		while( res < 0 ) {
			res = res + (2*Math.PI);
		}
		return res;
	}
			
	private void colisionRobot() throws IOException{	
		
		ArrayList<Integer> enemigos = managerObjetos.getEnemigosActivos();
		//System.out.println("malos: " + ParametrosGlobales.enemigos.length);
		
		if(enemigos != null) {
			
			for (int i = 0; i < enemigos.size(); i++) {
				//Point3d enemigo_actual = posicion_otros[enemigos.get(i)];
				Point3d enemigo_actual = control.entorno.getCoordenadas(enemigos.get(i));
							
				if(enemigo_actual != null){				
					double distancia_al_enemigo = u.distanciaEuclidea(mi_posicion,enemigo_actual);
					
								
					if( distancia_al_enemigo> 0 && distancia_al_enemigo < ParametrosGlobales.TOLERANCIA_ENEMIGOS ) {
						System.out.println("Entre en la tolerancia");
						System.out.println("Distancia al enemigo "+ i + ": " + distancia_al_enemigo);										
					}		
						
					double mi_angulo = normalizarAngulo(mi_posicion.z);
					
					/*VERIFICO SI CHOQUE FRONTALMENTE A UN ENEMIGO*/
					if( distancia_al_enemigo> 0 && distancia_al_enemigo < ParametrosGlobales.TOLERANCIA_ENEMIGOS &&
							angulosIguales(mi_posicion.z + Math.PI, enemigo_actual.z)) {
						
						if( (mi_posicion.y < enemigo_actual.y && mi_angulo >= 0 && mi_angulo < Math.PI )||
								(mi_posicion.y > enemigo_actual.y && mi_angulo < (2*Math.PI) && mi_angulo >= Math.PI )) {
	
							System.out.println("Choque frontalmente con un enemigo");
							
							control.entorno.enviarBeep();
							
							//Pauso tactica actual										
							//pausarDemonios();
							pausarEstrategia();
							
							//Retrocedo
							control.retroceder(ParametrosGlobales.VELOCIDAD_ESCAPE);
							try {
								sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO);
							} catch (InterruptedException e) {
								// TODO Auto-generated catch block
								e.printStackTrace();
							}
							control.detenerMotores();
							
							/*
							 * NO VOY A GIRAR CONTRA LOS ROBOTS ENEMIGOS
							//Giro
							double angulo_giro = (mi_posicion.z + ParametrosGlobales.ANGULO_ESCAPE) % (2*Math.PI);
							control.girar(angulo_giro);
							*/
							
							//Reanudo la tactica actual
							//reactivarDemonios();
							reanudarEstrategia();
						}
					}
					
					//VERIFICO SI UN ENEMIGO ME CHOCO LATERALMENTE//
					
					//Controlo que el enemigo este en una bola alrededor
					if( distancia_al_enemigo> 0 && distancia_al_enemigo < ParametrosGlobales.TOLERANCIA_DISTANCIA_MUERTE ) {
//*						System.out.println("Estoy en la toleracia de muerte");
//*						System.out.println("Distancia al enemigo "+ i + ": " + distancia_al_enemigo);
										
						//Usamos 45 +/- grados de tolerancia en el ataque
						int cuadrante = u.getCuadranteMuerte(enemigo_actual, mi_posicion);
						
						Point3d mi_posicionAux = new Point3d(mi_posicion.x, mi_posicion.y, mi_posicion.z - (Math.PI/2));
						int cuadranteAux = u.getCuadranteMuerte(enemigo_actual, mi_posicionAux);
						double angulo_enemigo_aux = enemigo_actual.z + (2*Math.PI);
						
						if(cuadrante == 1 && cuadranteAux == 1 || cuadrante == 3 && cuadranteAux == 3) {
							double d1 = u.getDiferenciaAngulos(mi_posicion.z, enemigo_actual.z);
							double d2 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI/2), enemigo_actual.z);
							if(d2 <= d1) {								
								//morir();
								me_mataron = true;
							} else {
								d1 = u.getDiferenciaAngulos(mi_posicion.z + (3*Math.PI/2), enemigo_actual.z);
								d2 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI), enemigo_actual.z);
								if(d1 <= d2)
									//morir();
									me_mataron = true;
							}
						} else if(cuadrante == 1 && cuadranteAux == 2 || cuadrante == 3 && cuadranteAux == 4) {
							double d3 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI/2), enemigo_actual.z);
							double d4 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI), enemigo_actual.z);
							if(d3 <= d4) {
								//morir();
								me_mataron = true;
							} else {
								d3 = u.getDiferenciaAngulos(mi_posicion.z + (3*Math.PI/2), enemigo_actual.z);
								d4 = u.getDiferenciaAngulos(mi_posicion.z, enemigo_actual.z);
								if(d3 <= d4)
									//morir();
									me_mataron = true;
							}
						} else if( cuadrante == 4 && cuadranteAux == 1 || cuadrante == 2 && cuadranteAux == 3 ) {
							double d1 = u.getDiferenciaAngulos(mi_posicion.z, angulo_enemigo_aux);
							double d2 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI/2), angulo_enemigo_aux);
							if(d1 <= d2) {
								//Murio el robot que ataque, le doy espacio para que gire
								//dejarEspacioParaMorir(cuadrante);
								mate_enemigo = true;
								cuadrante_mate_enemigo = cuadrante;
							} else {
								d1 = u.getDiferenciaAngulos(mi_posicion.z + (3*Math.PI/2), angulo_enemigo_aux);
								d2 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI), angulo_enemigo_aux);
								if(d1 >= d2){
									//Murio el robot que ataque, le doy espacio para que gire
									//dejarEspacioParaMorir(cuadrante);
									mate_enemigo = true;
									cuadrante_mate_enemigo = cuadrante;
								}
							}
						} else if(cuadrante == 4 && cuadranteAux == 4 || cuadrante == 2 && cuadranteAux == 2) {
							double d3 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI/2), angulo_enemigo_aux);
							double d4 = u.getDiferenciaAngulos(mi_posicion.z + (Math.PI), angulo_enemigo_aux);
							if(d3 >= d4) {
								//Murio el robot que ataque, le doy espacio para que gire
								//dejarEspacioParaMorir(cuadrante);
								mate_enemigo = true;
								cuadrante_mate_enemigo = cuadrante;
							} else {
								d3 = u.getDiferenciaAngulos(mi_posicion.z + (3*Math.PI/2), angulo_enemigo_aux);
								d4 = u.getDiferenciaAngulos(mi_posicion.z, angulo_enemigo_aux);
								if(d3 >= d4){
									//Murio el robot que ataque, le doy espacio para que gire
									//dejarEspacioParaMorir(cuadrante);
									mate_enemigo = true;
									cuadrante_mate_enemigo = cuadrante;
								}
							}
						}
					}				
				} 
			}
		}
	}
	
	private void dejarEspacioParaMorir(int cuadrante) {
		//Pedimos los motores
		pausarEstrategia();
		
		System.out.println("Aparentemente lo mate, esta en mi cuadrante " + cuadrante);
		try {
			control.detenerMotores();			
			switch( cuadrante ) {
			case 2:
				//Avanzo
				control.avanzar(ParametrosGlobales.VELOCIDAD_ESCAPE);				
				break;				
			case 4:		
				//Retrocedo
				control.retroceder(ParametrosGlobales.VELOCIDAD_ESCAPE);				
				break;			
			}						
			sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO*2);			
			control.detenerMotores();			
			sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO);		
			control.entorno.enviarBeep();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			try {
				control.detenerMotores();
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}
			e.printStackTrace();
		}

		//Devolemos los motores
		reanudarEstrategia();
	}
	
		
	private void pausarEstrategia() {
		//Tomo el control sobre la estrategia
		control.puedo_usar_motores = false;		
		
		//Detengo el control de movimiento		
		try {
			control.detenerMotores();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	private void reanudarEstrategia() {
		//Retomo el control de la estrategia
		control.puedo_usar_motores = true;
	}
	
	private void matarEstrategia() {
		//Elimino la estrategia
		murioRobot();
	}
	
	/*
	private void pausarDemonios() {
		
		//Pausando los demonios de la interfaz	
		System.out.println("PARANDO DEMONIOS..............");
		Iterator<FuncionalidadD> iteradorB = Principal.listaDaemons.iterator();
		while(iteradorB.hasNext()) {			
			System.out.println("holas");
			FuncionalidadD thread = iteradorB.next();
			//iteradorB.remove();
			//thread.stop();
			thread.suspend();
		}		
		System.out.println("OK");		
	}
	
	private void reactivarDemonios() {		
		System.out.println("REACTIVANDO DEMONIOS..............");
		Iterator<FuncionalidadD> iteradorB = Principal.listaDaemons.iterator();
		while(iteradorB.hasNext()) {			
			System.out.println("holas");
			FuncionalidadD thread = iteradorB.next();
			//iteradorB.remove();
			//thread.stop();
			thread.resume();
		}
		System.out.println("OK");		
	}
	
	private void matarDemonios() {
		System.out.println("PARANDO DEMONIOS..............");
		Iterator<FuncionalidadD> iteradorB = Principal.listaDaemons.iterator();
		while(iteradorB.hasNext()) {			
			System.out.println("holas");
			FuncionalidadD thread = iteradorB.next();
			//iteradorB.remove();
			//thread.stop();
			thread.suspend();
		}		
		System.out.println("OK");
	}
	*/
	
	private void colisionFuerte() throws IOException{
		
		
		ArrayList<Integer> fuertes = managerObjetos.getFuertes();
		
		if(fuertes != null) {
			
		
			for (int i = 0; i < fuertes.size(); i++) {			
				
				//Point3d fuerte_actual = posicion_otros[fuertes.get(i)];
				Point3d fuerte_actual = control.entorno.getCoordenadas(fuertes.get(i));
							
				double distancia_al_fuerte = u.distanciaEuclidea(mi_posicion,fuerte_actual);
				
				if( distancia_al_fuerte > 0 && distancia_al_fuerte < ParametrosGlobales.TOLERANCIA_FUERTES ) {
					
					control.entorno.enviarBeep();
					
					//Pauso tactica actual										
					//pausarDemonios();
					pausarEstrategia();
					
					//Retrocedo
					control.retroceder(ParametrosGlobales.VELOCIDAD_ESCAPE);
					try {
						sleep(ParametrosGlobales.TIEMPO_DE_RETROCESO);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
					control.detenerMotores();
					//Giro
					double angulo_giro = (mi_posicion.z + ParametrosGlobales.ANGULO_ESCAPE) % (2*Math.PI);
					control.girar(angulo_giro);
					
					//Reanudo tactica actual
					//reactivarDemonios();
					reanudarEstrategia();
				}
			}
		}
	}
	
	/**
	 * Giro 2 vueltas y lanzo un beep.
	 * */
	private void morir() {
		System.out.println("MORI");
		//matarDemonios();
		pausarEstrategia();
		double angulo_actual;
		try {
			control.detenerMotores();
			for (int i = 0; i < 4; i++) {
				angulo_actual = control.getMiPosicion().z;			
				control.girar(angulo_actual - (3.14));
			}
			//Giro un poco mas para llegar a las 2 vueltas completas
			angulo_actual = control.getMiPosicion().z;
			control.girar(angulo_actual - (1.1));
			sleep(ParametrosGlobales.TIEMPO_ENTRE_CONTROLES);			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} finally {
			try {
				control.detenerMotores();
				control.entorno.enviarBeep();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		//reanudarEstrategia();
		matarEstrategia();
		try {
			control.entorno.detenerMotores();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	
	@Override
	public void run() {
		/*
		Point3d p1 = new Point3d(0.5,0.57,0.13);
		Point3d p2 = new Point3d(0.67,0.597,-1.63);
			
		int cuadrante = u.getCuadranteMuerte(p1, p2);
		System.out.println("Cuadrante = " + cuadrante);
		
		p1 = new Point3d(0.5,0.5,0);
		
		p2 = new Point3d(0.6,0.5,0);
		cuadrante = u.getCuadranteMuerte(p2, p1);
		System.out.println("Cuadrante = " + cuadrante);
		p2 = new Point3d(0.5,0.6,0);
		cuadrante = u.getCuadranteMuerte(p2, p1);
		System.out.println("Cuadrante = " + cuadrante);
		
		p2 = new Point3d(0.4,0.51,0);
		cuadrante = u.getCuadranteMuerte(p2, p1);
		System.out.println("Cuadrante = " + cuadrante);
		p2 = new Point3d(0.4,0.49,0);
		cuadrante = u.getCuadranteMuerte(p2, p1);
		System.out.println("Cuadrante = " + cuadrante);
		
		p2 = new Point3d(0.5,0.4,0);
		cuadrante = u.getCuadranteMuerte(p2, p1);
		System.out.println("Cuadrante = " + cuadrante);
		*/
		
		//morir();
		int contador = 0;
		int contador_me_mataron = 0;
		while (vivo) {
			mate_enemigo = false;
			
			if( robotVivo()) {
				try {
					sleep(ParametrosGlobales.TIEMPO_ENTRE_CONTROLES);
					mi_posicion = control.getMiPosicion();
				//	posicion_otros = control.getPosicionesObstaculos();
					colisionPared();
					//mi_posicion = control.getMiPosicion();
				//	posicion_otros = control.getPosicionesObstaculos();
					//colisionFuerte();
					//mi_posicion = control.getMiPosicion();
				//	posicion_otros = control.getPosicionesObstaculos();
					colisionRobot();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
				if(mate_enemigo) {
					contador++;
				} else {
					contador = 0;
				}
				
				if(me_mataron){
					contador_me_mataron++;
				} else {
					contador_me_mataron = 0;
				}
				
				if(contador * ParametrosGlobales.TIEMPO_ENTRE_CONTROLES > 1000) {
					dejarEspacioParaMorir(cuadrante_mate_enemigo);
					contador = 0;
				}
				if(contador_me_mataron * ParametrosGlobales.TIEMPO_ENTRE_CONTROLES > 1000) {
					morir();
					//contador_me_mataron = 0;
				}
			}
		}
	}
}
